
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_guided.c
  * @author     baiyang
  * @date       2022-4-18
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"

#include <mavproxy/mavproxy.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool mguided_init(mode_base_t mode, bool ignore_checks);
static void mguided_run(mode_base_t mode);
static bool mguided_allows_arming(mode_base_t mode, ArmingMethod method);
static bool mguided_do_user_takeoff_start(mode_base_t mode, float takeoff_alt_cm);
static void mguided_pva_control_start(mode_base_t mode);
static void mguided_pos_control_start(mode_base_t mode);
static void mguided_accel_control_start(mode_base_t mode);
static void mguided_velaccel_control_start(mode_base_t mode);
static void mguided_posvelaccel_control_start(mode_base_t mode);
static bool mguided_is_taking_off(mode_base_t mode);
static bool mguided_get_wp(mode_base_t mode, Location* destination);
static void mguided_takeoff_run(mode_base_t mode);
static void mguided_pos_control_run(mode_base_t mode);
static void mguided_accel_control_run(mode_base_t mode);
static void mguided_velaccel_control_run(mode_base_t mode);
static void mguided_pause_control_run(mode_base_t mode);
static void mguided_posvelaccel_control_run(mode_base_t mode);
static void mguided_set_yaw_state(mode_base_t mode, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);

static ModeNumber mguided_number(mode_base_t mode);
static const char *mguided_name(mode_base_t mode);
static const char *mguided_name4(mode_base_t mode);
static bool mguided_requires_GPS(mode_base_t mode);
static bool mguided_has_manual_throttle(mode_base_t mode);
static bool mguided_is_autopilot(mode_base_t mode);
static bool mguided_has_user_takeoff(mode_base_t mode, bool must_navigate);
static bool mguided_in_guided_mode(mode_base_t mode);
static bool mguided_requires_terrain_failsafe(mode_base_t mode);
static bool mguided_use_pilot_yaw(mode_base_t mode);
static bool mguided_pause(mode_base_t mode);
static bool mguided_resume(mode_base_t mode);
static uint32_t mguided_wp_distance(mode_base_t mode);
static int32_t mguided_wp_bearing(mode_base_t mode);
static float mguided_crosstrack_error(mode_base_t mode);
/*----------------------------------variable----------------------------------*/
static Vector3p guided_pos_target_cm;       // position target (used by posvel controller only)
static bool guided_pos_terrain_alt;                // true if guided_pos_target_cm.z is an alt above terrain
static Vector3f_t guided_vel_target_cms;      // velocity target (used by pos_vel_accel controller and vel_accel controller)
static Vector3f_t guided_accel_target_cmss;   // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller)
static uint32_t update_time_ms;             // system time of last target update to pos_vel_accel, vel_accel or accel controller

struct {
    uint32_t update_time_ms;
    Quat_t attitude_quat;
    Vector3f_t ang_vel;
    float yaw_rate_cds;
    float climb_rate_cms;   // climb rate in cms.  Used if use_thrust is false
    float thrust;           // thrust from -1 to 1.  Used if use_thrust is true
    bool use_yaw_rate;
    bool use_thrust;
} static guided_angle_state;

struct Guided_Limit {
    uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
    float alt_min_cm;   // lower altitude limit in cm above home (0 = no limit)
    float alt_max_cm;   // upper altitude limit in cm above home (0 = no limit)
    float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
    uint32_t start_time;// system time in milliseconds that control was handed to the external computer
    Vector3f_t start_pos; // start position as a distance from home in cm.  used for checking horiz_max limit
} guided_limit;

static struct mode_ops mode_guided_ops = {
        .mode_number = mguided_number,
        .init        = mguided_init,
        .exit        = NULL,
        .run         = mguided_run,
        .requires_GPS = mguided_requires_GPS,
        .has_manual_throttle = mguided_has_manual_throttle,
        .allows_arming = mguided_allows_arming,
        .is_autopilot = mguided_is_autopilot,
        .has_user_takeoff = mguided_has_user_takeoff,
        .in_guided_mode = mguided_in_guided_mode,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = NULL,
        .allows_flip = NULL,
        .name = mguided_name,
        .name4 = mguided_name4,
        .is_taking_off = mguided_is_taking_off,
        .is_landing = NULL,
        .requires_terrain_failsafe = mguided_requires_terrain_failsafe,
        .get_wp = mguided_get_wp,
        .wp_bearing = mguided_wp_bearing,
        .wp_distance = mguided_wp_distance,
        .crosstrack_error = mguided_crosstrack_error,
        .output_to_motors = NULL,
        .use_pilot_yaw = mguided_use_pilot_yaw,
        .throttle_hover = NULL,
        .do_user_takeoff_start = mguided_do_user_takeoff_start,
        .pause = mguided_pause,
        .resume = mguided_resume};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mode_guided_ctor(ModeGuided* mode_guided)
{
    mode_ctor(&mode_guided->mode, &mode_guided_ops);

    mode_guided->guided_mode = GUIDED_TakeOff;
}

// return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rates
uint32_t mguided_get_timeout_ms()
{
    return MAX(fms.g.guided_timeout, 0.1) * 1000;
}

// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
bool mguided_set_attitude_target_provides_thrust()
{
    return ((fms.g.guided_options & (uint32_t)(GuidedSetAttitudeTarget_ThrustAsThrust)) != 0);
}

// returns true if GUIDED_OPTIONS param specifies position should be controlled (when velocity and/or acceleration control is active)
bool mguided_stabilizing_pos_xy()
{
    return !((fms.g.guided_options & (uint32_t)(GuidedDoNotStabilizePositionXY)) != 0);
}

// returns true if GUIDED_OPTIONS param specifies velocity should  be controlled (when acceleration control is active)
bool mguided_stabilizing_vel_xy()
{
    return !((fms.g.guided_options & (uint32_t)(GuidedDoNotStabilizeVelocityXY)) != 0);
}

// returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower)
bool mguided_use_wpnav_for_position_control()
{
    return ((fms.g.guided_options & (uint32_t)(GuidedWPNavUsedForPosControl)) != 0);
}

// initialise guided mode's angle controller
void mguided_angle_control_start(mode_base_t mode)
{
    // set guided_mode to velocity controller
    ((ModeGuided*)mode)->guided_mode = GUIDED_Angle;

    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));
    posctrl_set_correction_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));

    // initialise the vertical position controller
    if (!posctrl_is_active_z(mode->pos_control)) {
        posctrl_init_z_controller(mode->pos_control);
    }

    // initialise targets
    guided_angle_state.update_time_ms = time_millis();
    quat_init(&guided_angle_state.attitude_quat);
    vec3_zero(&guided_angle_state.ang_vel);
    guided_angle_state.climb_rate_cms = 0.0f;
    guided_angle_state.yaw_rate_cds = 0.0f;
    guided_angle_state.use_yaw_rate = false;

    // pilot always controls yaw
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
}

// angle_control_run - runs the guided angle controller
// called from guided_run
void mguided_angle_control_run(mode_base_t mode)
{
    float climb_rate_cms = 0.0f;
    if (!guided_angle_state.use_thrust) {
        // constrain climb rate
        climb_rate_cms = math_constrain_float(guided_angle_state.climb_rate_cms, -wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav));

        // get avoidance adjusted climb rate
        //climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
    }

    // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
    uint32_t tnow = time_millis();
    if (tnow - guided_angle_state.update_time_ms > mguided_get_timeout_ms()) {
        quat_init(&guided_angle_state.attitude_quat);
        vec3_zero(&guided_angle_state.ang_vel);
        climb_rate_cms = 0.0f;
        if (guided_angle_state.use_thrust) {
            // initialise vertical velocity controller
            posctrl_init_z_controller(mode->pos_control);
            guided_angle_state.use_thrust = false;
        }
    }

    // interpret positive climb rate or thrust as triggering take-off
    const bool positive_thrust_or_climbrate = math_flt_positive(guided_angle_state.use_thrust ? guided_angle_state.thrust : climb_rate_cms);
    if (mode->motors->_armed && positive_thrust_or_climbrate) {
        fms_set_auto_armed(true);
    }

    // if not armed set throttle to zero and exit immediately
    if (!mode->motors->_armed || !fms.ap.auto_armed || (fms.ap.land_complete && !positive_thrust_or_climbrate)) {
        // do not spool down tradheli when on the ground with motor interlock enabled
        // copter.is_tradheli() && motors->get_interlock()
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // TODO: use get_alt_hold_state
    // landed with positive desired climb rate, takeoff
    if (fms.ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) {
        mode_zero_throttle_and_relax_ac(false);
        Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
        if (mode->motors->_spool_state == MOTOR_THROTTLE_UNLIMITED) {
            fms_set_land_complete(false);
            mode_set_throttle_takeoff();
        }
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // call attitude controller
    if (quat_is_zero(&guided_angle_state.attitude_quat)) {
        attctrl_input_rate_bf_roll_pitch_yaw(mode->attitude_control, ToDeg(guided_angle_state.ang_vel.x) * 100.0f, ToDeg(guided_angle_state.ang_vel.y) * 100.0f, ToDeg(guided_angle_state.ang_vel.z) * 100.0f);
    } else {
        attctrl_input_quaternion(mode->attitude_control, &guided_angle_state.attitude_quat, guided_angle_state.ang_vel);
    }

    // call position controller
    if (guided_angle_state.use_thrust) {
        attctrl_set_throttle_out(mode->attitude_control, guided_angle_state.thrust, true, fms.g.throttle_filt);
    } else {
        posctrl_set_pos_target_z_from_climb_rate_cm(mode->pos_control, climb_rate_cms);
        posctrl_update_z_controller(mode->pos_control);
    }
}

// set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool mguided_set_destination(mode_base_t mode, const Vector3f_t* destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt)
{
    // if configured to use position controller for position control
    // ensure we are in position control mode
    if (((ModeGuided*)mode)->guided_mode != GUIDED_Pos) {
        mguided_pos_control_start(mode);
    }

    // initialise terrain following if needed
    if (terrain_alt) {
        // get current alt above terrain
        float origin_terr_offset;
        if (!wpnav_get_terrain_offset(mode->wp_nav, &origin_terr_offset)) {
            // if we don't have terrain altitude then stop
            mode_init(mode, true);
            return false;
        }
        // convert origin to alt-above-terrain if necessary
        if (!guided_pos_terrain_alt) {
            // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
            posctrl_set_pos_offset_z_cm(mode->pos_control, origin_terr_offset);
        }
    } else {
        posctrl_set_pos_offset_z_cm(mode->pos_control, 0.0f);
    }

    // set yaw state
    mguided_set_yaw_state(mode, use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

    // set position target and zero velocity and acceleration
    guided_pos_target_cm.x = destination->x;
    guided_pos_target_cm.y = destination->y;
    guided_pos_target_cm.z = destination->z;

    guided_pos_terrain_alt = terrain_alt;
    vec3_zero(&guided_vel_target_cms);
    vec3_zero(&guided_accel_target_cmss);
    update_time_ms = time_millis();

    // log target
    //copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);

    ((ModeGuided*)mode)->send_notification = true;

    return true;
}

// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool mguided_set_destination2(mode_base_t mode, const Location* dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
    // if configured to use position controller for position control
    // ensure we are in position control mode
    if (((ModeGuided*)mode)->guided_mode != GUIDED_Pos) {
        mguided_pos_control_start(mode);
    }

    // set yaw state
    mguided_set_yaw_state(mode, use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

    // set position target and zero velocity and acceleration
    Vector3f_t pos_target_f = VECTOR3F_DEFAULT_VALUE;
    bool terrain_alt;
    if (!wpnav_get_vector_NEU(dest_loc, &pos_target_f, &terrain_alt)) {
        return false;
    }

    // initialise terrain following if needed
    if (terrain_alt) {
        // get current alt above terrain
        float origin_terr_offset;
        if (!wpnav_get_terrain_offset(mode->wp_nav, &origin_terr_offset)) {
            // if we don't have terrain altitude then stop
            mode_init(mode, true);
            return false;
        }
        // convert origin to alt-above-terrain if necessary
        if (!guided_pos_terrain_alt) {
            // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
            posctrl_set_pos_offset_z_cm(mode->pos_control, origin_terr_offset);
        }
    } else {
        posctrl_set_pos_offset_z_cm(mode->pos_control, 0.0f);
    }

    guided_pos_target_cm.x = pos_target_f.x;
    guided_pos_target_cm.y = pos_target_f.y;
    guided_pos_target_cm.z = pos_target_f.z;

    guided_pos_terrain_alt = terrain_alt;
    vec3_zero(&guided_vel_target_cms);
    vec3_zero(&guided_accel_target_cmss);
    update_time_ms = time_millis();

    // log target
    //copter.Log_Write_Guided_Position_Target(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);

    ((ModeGuided*)mode)->send_notification = true;

    return true;
}

// set_velaccel - sets guided mode's target velocity and acceleration
void mguided_set_accel(mode_base_t mode, const Vector3f_t* acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
{
    // check we are in velocity control mode
    if (((ModeGuided*)mode)->guided_mode != GUIDED_Accel) {
        mguided_accel_control_start(mode);
    }

    // set yaw state
    mguided_set_yaw_state(mode, use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

    // set velocity and acceleration targets and zero position
    vec3_zero(&guided_pos_target_cm);
    guided_pos_terrain_alt = false;
    vec3_zero(&guided_vel_target_cms);
    guided_accel_target_cmss = *acceleration;
    update_time_ms = time_millis();

    // log target
    if (log_request) {
        //copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
    }
}

// set_velocity - sets guided mode's target velocity
void mguided_set_velocity(mode_base_t mode, const Vector3f_t* velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
{
    Vector3f_t acceleration = {0.0f, 0.0f, 0.0f};
    mguided_set_velaccel(mode, velocity, &acceleration, use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw, log_request);
}

// set_velaccel - sets guided mode's target velocity and acceleration
void mguided_set_velaccel(mode_base_t mode, const Vector3f_t* velocity, const Vector3f_t* acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
{
    // check we are in velocity control mode
    if (((ModeGuided*)mode)->guided_mode != GUIDED_VelAccel) {
        mguided_velaccel_control_start(mode);
    }

    // set yaw state
    mguided_set_yaw_state(mode, use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

    // set velocity and acceleration targets and zero position
    vec3_zero(&guided_pos_target_cm);
    guided_pos_terrain_alt = false;
    guided_vel_target_cms = *velocity;
    guided_accel_target_cmss = *acceleration;
    update_time_ms = time_millis();

    // log target
    if (log_request) {
        //copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
    }
}

// set_destination_posvel - set guided mode position and velocity target
bool mguided_set_destination_posvel(mode_base_t mode, const Vector3f_t* destination, const Vector3f_t* velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
    Vector3f_t acceleration = {0.0f, 0.0f, 0.0f};
    return mguided_set_destination_posvelaccel(mode, destination, velocity, &acceleration, use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
}

// set_destination_posvelaccel - set guided mode position, velocity and acceleration target
bool mguided_set_destination_posvelaccel(mode_base_t mode, const Vector3f_t* destination, const Vector3f_t* velocity, const Vector3f_t* acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
    // check we are in velocity control mode
    if (((ModeGuided*)mode)->guided_mode != GUIDED_PosVelAccel) {
        mguided_posvelaccel_control_start(mode);
    }

    // set yaw state
    mguided_set_yaw_state(mode, use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

    update_time_ms = time_millis();
    guided_pos_target_cm.x = destination->x;
    guided_pos_target_cm.y = destination->y;
    guided_pos_target_cm.z = destination->z;

    guided_pos_terrain_alt = false;
    guided_vel_target_cms = *velocity;
    guided_accel_target_cmss = *acceleration;

    // log target
    //copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
    return true;
}

// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
//                IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
// ang_vel: angular velocity (rad/s)
// climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
// use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
//             IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
void mguided_set_angle(mode_base_t mode, const Quat_t *attitude_quat, const Vector3f_t *ang_vel, float climb_rate_cms_or_thrust, bool use_thrust)
{
    // check we are in velocity control mode
    if (((ModeGuided*)mode)->guided_mode != GUIDED_Angle) {
        mguided_angle_control_start(mode);
    }

    guided_angle_state.attitude_quat = *attitude_quat;
    guided_angle_state.ang_vel = *ang_vel;

    guided_angle_state.use_thrust = use_thrust;
    if (use_thrust) {
        guided_angle_state.thrust = climb_rate_cms_or_thrust;
        guided_angle_state.climb_rate_cms = 0.0f;
    } else {
        guided_angle_state.thrust = 0.0f;
        guided_angle_state.climb_rate_cms = climb_rate_cms_or_thrust;
    }

    guided_angle_state.update_time_ms = time_millis();

    // convert quaternion to euler angles
    float roll_rad, pitch_rad, yaw_rad;
    quat_to_euler2(attitude_quat, &roll_rad, &pitch_rad, &yaw_rad);

    // log target
    //copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01);
}

// limit_clear - clear/turn off guided limits
void mguided_limit_clear()
{
    guided_limit.timeout_ms = 0;
    guided_limit.alt_min_cm = 0.0f;
    guided_limit.alt_max_cm = 0.0f;
    guided_limit.horiz_max_cm = 0.0f;
}

// limit_set - set guided timeout and movement limits
void mguided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
{
    guided_limit.timeout_ms = timeout_ms;
    guided_limit.alt_min_cm = alt_min_cm;
    guided_limit.alt_max_cm = alt_max_cm;
    guided_limit.horiz_max_cm = horiz_max_cm;
}

// limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
//  only called from AUTO mode's auto_nav_guided_start function
void mguided_limit_init_time_and_pos()
{
    // initialise start time
    guided_limit.start_time = time_millis();

    // initialise start position from current position
    guided_limit.start_pos = fms.ahrs->relpos_cm;
}

// limit_check - returns true if guided mode has breached a limit
//  used when guided is invoked from the NAV_GUIDED_ENABLE mission command
bool mguided_limit_check()
{
    // check if we have passed the timeout
    if ((guided_limit.timeout_ms > 0) && (time_millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
        return true;
    }

    // get current location
    const Vector3f_t* curr_pos = &fms.ahrs->relpos_cm;

    // check if we have gone below min alt
    if (!math_flt_zero(guided_limit.alt_min_cm) && (curr_pos->z < guided_limit.alt_min_cm)) {
        return true;
    }

    // check if we have gone above max alt
    if (!math_flt_zero(guided_limit.alt_max_cm) && (curr_pos->z > guided_limit.alt_max_cm)) {
        return true;
    }

    // check if we have gone beyond horizontal limit
    if (guided_limit.horiz_max_cm > 0.0f) {
        const float horiz_move = vec3_get_horizontal_distance_cm(&guided_limit.start_pos, curr_pos);
        if (horiz_move > guided_limit.horiz_max_cm) {
            return true;
        }
    }

    // if we got this far we must be within limits
    return false;
}

const Vector3p* mguided_get_target_pos()
{
    return &guided_pos_target_cm;
}

const Vector3f_t* mguided_get_target_vel()
{
    return &guided_vel_target_cms;
}

const Vector3f_t* mguided_get_target_accel()
{
    return &guided_accel_target_cmss;
}

// init - initialise guided controller
static bool mguided_init(mode_base_t mode, bool ignore_checks)
{
    ModeGuided* mode_guided = (ModeGuided*)mode;

    // start in velaccel control mode
    mguided_velaccel_control_start(mode);
    vec3_zero(&guided_vel_target_cms);
    vec3_zero(&guided_accel_target_cmss);
    mode_guided->send_notification = false;

    // clear pause state when entering guided mode
    mode_guided->_paused = false;

    return true;
}

// run - runs the guided controller
// should be called at 100hz or more
static void mguided_run(mode_base_t mode)
{
    // run pause control if the vehicle is paused
    if (((ModeGuided*)mode)->_paused) {
        mguided_pause_control_run(mode);
        return;
    }

    // call the correct auto controller
    switch (((ModeGuided*)mode)->guided_mode) {

    case GUIDED_TakeOff:
        // run takeoff controller
        mguided_takeoff_run(mode);
        break;

    case GUIDED_Pos:
    case GUIDED_WP:
        // run position controller
        mguided_pos_control_run(mode);
        break;

    case GUIDED_Accel:
        mguided_accel_control_run(mode);
        break;

    case GUIDED_VelAccel:
        mguided_velaccel_control_run(mode);
        break;

    case GUIDED_PosVelAccel:
        mguided_posvelaccel_control_run(mode);
        break;

    case GUIDED_Angle:
        mguided_angle_control_run(mode);
        break;
    }

}

static bool mguided_allows_arming(mode_base_t mode, ArmingMethod method)
{
    // always allow arming from the ground station
    if (method == ARMING_CHECK_MAVLINK) {
        return true;
    }

    // optionally allow arming from the transmitter
    return (fms.g.guided_options & (uint32_t)GuidedAllowArmingFromTX) != 0;
};

// initialises position controller to implement take-off
// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
static bool mguided_do_user_takeoff_start(mode_base_t mode, float takeoff_alt_cm)
{
    // calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain)
    int32_t alt_target_cm;
    bool alt_target_terrain = false;
#if 0
    if (wpnav_rangefinder_used_and_healthy(mode->wp_nav) &&
        wpnav_get_terrain_source(mode->wp_nav) == WPNAV_TERRAIN_FROM_RANGEFINDER &&
        takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
        // can't takeoff downwards
        if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
            return false;
        }
        // provide target altitude as alt-above-terrain
        alt_target_cm = takeoff_alt_cm;
        alt_target_terrain = true;
    } else {
        // interpret altitude as alt-above-home
        Location target_loc = copter.current_loc;
        target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME);

        // provide target altitude as alt-above-ekf-origin
        if (!target_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
            // this should never happen but we reject the command just in case
            return false;
        }
    }
#endif

    // interpret altitude as alt-above-home
    Location target_loc = mode->ahrs->curr_loc;
    location_set_alt_cm(&target_loc, takeoff_alt_cm, ALT_FRAME_ABOVE_HOME);

    // provide target altitude as alt-above-ekf-origin
    if (!location_get_alt_cm(&target_loc, ALT_FRAME_ABOVE_ORIGIN, &alt_target_cm)) {
        // this should never happen but we reject the command just in case
        return false;
    }

    ((ModeGuided*)mode)->guided_mode = GUIDED_TakeOff;

    // initialise yaw
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);

    // clear i term when we're taking off
    mode_set_throttle_takeoff();

    // initialise alt for WP_NAVALT_MIN and set completion alt
    mode_auto_takeoff_start(mode, alt_target_cm, alt_target_terrain);

    // record takeoff has not completed
    ((ModeGuided*)mode)->takeoff_complete = false;

    return true;
}

// initialise position controller
static void mguided_pva_control_start(mode_base_t mode)
{
    // initialise horizontal speed, acceleration
    posctrl_set_max_speed_accel_xy(mode->pos_control, wpnav_get_default_speed_xy(mode->wp_nav), wpnav_get_wp_acceleration(mode->wp_nav));
    posctrl_set_correction_speed_accel_xy(mode->pos_control, wpnav_get_default_speed_xy(mode->wp_nav), wpnav_get_wp_acceleration(mode->wp_nav));

    // initialize vertical speeds and acceleration
    posctrl_set_max_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));
    posctrl_set_correction_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));

    // initialise velocity controller
    posctrl_init_z_controller(mode->pos_control);
    posctrl_init_xy_controller(mode->pos_control);

    // initialise yaw
    autoyaw_set_mode_to_default(mode->auto_yaw, false);

    // initialise terrain alt
    guided_pos_terrain_alt = false;
}

// initialise guided mode's position controller
static void mguided_pos_control_start(mode_base_t mode)
{
    // set to position control mode
    ((ModeGuided*)mode)->guided_mode = GUIDED_Pos;

    // initialise position controller
    mguided_pva_control_start(mode);
}

// initialise guided mode's velocity controller
static void mguided_accel_control_start(mode_base_t mode)
{
    // set guided_mode to velocity controller
    ((ModeGuided*)mode)->guided_mode = GUIDED_Accel;

    // initialise position controller
    mguided_pva_control_start(mode);
}

// initialise guided mode's velocity and acceleration controller
static void mguided_velaccel_control_start(mode_base_t mode)
{
    // set guided_mode to velocity controller
    ((ModeGuided*)mode)->guided_mode = GUIDED_VelAccel;

    // initialise position controller
    mguided_pva_control_start(mode);
}

// initialise guided mode's position, velocity and acceleration controller
static void mguided_posvelaccel_control_start(mode_base_t mode)
{
    // set guided_mode to velocity controller
    ((ModeGuided*)mode)->guided_mode = GUIDED_PosVelAccel;

    // initialise position controller
    mguided_pva_control_start(mode);
}

static bool mguided_is_taking_off(mode_base_t mode)
{
    return ((ModeGuided*)mode)->guided_mode == GUIDED_TakeOff && !((ModeGuided*)mode)->takeoff_complete;
}

static bool mguided_get_wp(mode_base_t mode, Location* destination)
{
    Vector3f_t pos_target_cm = VECTOR3F_DEFAULT_VALUE;

    switch (((ModeGuided*)mode)->guided_mode) {
    case GUIDED_WP:
        return false;         ///wp_nav->get_oa_wp_destination(destination);
    case GUIDED_Pos:
        pos_target_cm.x = guided_pos_target_cm.x;
        pos_target_cm.y = guided_pos_target_cm.y;
        pos_target_cm.z = guided_pos_target_cm.z;
        Location_from_vector_xy_alt(destination, &pos_target_cm, guided_pos_terrain_alt ? ALT_FRAME_ABOVE_TERRAIN : ALT_FRAME_ABOVE_ORIGIN);
        return true;
    default:
        return false;
    }

    // should never get here but just in case
    return false;
}

// takeoff_run - takeoff in guided mode
//      called by guided_run at 100hz or more
static void mguided_takeoff_run(mode_base_t mode)
{
    mode_auto_takeoff_run(mode);
    if (*mode->auto_takeoff_complete && !((ModeGuided*)mode)->takeoff_complete) {
        ((ModeGuided*)mode)->takeoff_complete = true;
    }
}

// pos_control_run - runs the guided position controller
// called from guided_run
static void mguided_pos_control_run(mode_base_t mode)
{
    // process pilot's yaw input
    float target_yaw_rate = 0;

    if (!fms.failsafe.radio && mode_use_pilot_yaw(mode)) {
        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(mode->channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        // do not spool down tradheli when on the ground with motor interlock enabled
        // copter.is_tradheli() && motors->get_interlock()
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // calculate terrain adjustments
    float terr_offset = 0.0f;
    if (guided_pos_terrain_alt && !wpnav_get_terrain_offset(mode->wp_nav, &terr_offset)) {
        // failure to set destination can only be because of missing terrain data
        //copter.failsafe_terrain_on_event();
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // send position and velocity targets to position controller
    vec3_zero(&guided_accel_target_cmss);
    vec3_zero(&guided_vel_target_cms);

    // stop rotating if no updates received within timeout_ms
    if (time_millis() - update_time_ms > mguided_get_timeout_ms()) {
        if (((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_RATE) || ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_ANGLE_RATE)) {
            autoyaw_set_rate(mode->auto_yaw, 0.0f);
        }
    }

    float pos_offset_z_buffer = 0.0; // Vertical buffer size in m
    if (guided_pos_terrain_alt) {
        pos_offset_z_buffer = MIN(wpnav_get_terrain_margin(mode->wp_nav) * 100.0f, 0.5f * fabsf(guided_pos_target_cm.z));
    }
    posctrl_input_pos_xyz(mode->pos_control, &guided_pos_target_cm, terr_offset, pos_offset_z_buffer);

    // run position controllers
    posctrl_update_xy_controller(mode->pos_control);
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t posctrl_thrust_vector = posctrl_get_thrust_vector(mode->pos_control);

    // call attitude controller
    if ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_HOLD) {
        // roll & pitch from position controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, target_yaw_rate);
    } else if ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_RATE) {
        // roll & pitch from position controller, yaw rate from mavlink command or mission item
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_rate_cds(mode->auto_yaw));
    } else {
        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_yaw(mode->auto_yaw), autoyaw_rate_cds(mode->auto_yaw));
    }
}

// velaccel_control_run - runs the guided velocity controller
// called from guided_run
static void mguided_accel_control_run(mode_base_t mode)
{
    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!fms.failsafe.radio && mode_use_pilot_yaw(mode)) {
        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(mode->channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        // do not spool down tradheli when on the ground with motor interlock enabled
        // copter.is_tradheli() && motors->get_interlock()
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // set velocity to zero and stop rotating if no updates received for 3 seconds
    uint32_t tnow = time_millis();
    if (tnow - update_time_ms > mguided_get_timeout_ms()) {
        vec3_zero(&guided_vel_target_cms);
        vec3_zero(&guided_accel_target_cmss);
        if (((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_RATE) || ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_ANGLE_RATE)) {
            autoyaw_set_rate(mode->auto_yaw, 0.0f);
        }

        Vector2f_t guided_vel_target_cms_xy = {guided_vel_target_cms.x, guided_vel_target_cms.y};
        Vector2f_t guided_accel_target_cmss_xy = {guided_accel_target_cmss.x, guided_accel_target_cmss.y};

        posctrl_input_vel_accel_xy(mode->pos_control, &guided_vel_target_cms_xy, &guided_accel_target_cmss_xy, false);
        posctrl_input_vel_accel_z(mode->pos_control, &guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);
    } else {
        // update position controller with new target
        posctrl_input_accel_xy(mode->pos_control, &guided_accel_target_cmss);
        if (!mguided_stabilizing_vel_xy()) {
            // set position and velocity errors to zero
            posctrl_stop_vel_xy_stabilisation(mode->pos_control);
        } else if (!mguided_stabilizing_pos_xy()) {
            // set position errors to zero
            posctrl_stop_pos_xy_stabilisation(mode->pos_control);
        }
        posctrl_input_accel_z(mode->pos_control, guided_accel_target_cmss.z);
    }

    // call velocity controller which includes z axis controller
    posctrl_update_xy_controller(mode->pos_control);
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t posctrl_thrust_vector = posctrl_get_thrust_vector(mode->pos_control);

    // call attitude controller
    if ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_HOLD) {
        // roll & pitch from position controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, target_yaw_rate);
    } else if ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_RATE) {
        // roll & pitch from position controller, yaw rate from mavlink command or mission item
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_rate_cds(mode->auto_yaw));
    } else {
        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_yaw(mode->auto_yaw), autoyaw_rate_cds(mode->auto_yaw));
    }
}

// velaccel_control_run - runs the guided velocity and acceleration controller
// called from guided_run
static void mguided_velaccel_control_run(mode_base_t mode)
{
    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!fms.failsafe.radio && mode_use_pilot_yaw(mode)) {
        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(mode->channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        // do not spool down tradheli when on the ground with motor interlock enabled
        // copter.is_tradheli() && motors->get_interlock()
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // set velocity to zero and stop rotating if no updates received for 3 seconds
    uint32_t tnow = time_millis();
    if (tnow - update_time_ms > mguided_get_timeout_ms()) {
        vec3_zero(&guided_vel_target_cms);
        vec3_zero(&guided_accel_target_cmss);
        if (((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_RATE) || ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_ANGLE_RATE)) {
            autoyaw_set_rate(mode->auto_yaw, 0.0f);
        }
    }

    bool do_avoid = false;
#if 0
    // limit the velocity for obstacle/fence avoidance
    copter.avoid.adjust_velocity(guided_vel_target_cms, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt);
    do_avoid = copter.avoid.limits_active();
#endif

    // update position controller with new target

    if (!mguided_stabilizing_vel_xy() && !do_avoid) {
        // set the current commanded xy vel to the desired vel
        guided_vel_target_cms.x = posctrl_get_vel_desired_cms(mode->pos_control).x;
        guided_vel_target_cms.y = posctrl_get_vel_desired_cms(mode->pos_control).y;
    }

    Vector2f_t guided_vel_target_cms_xy = {guided_vel_target_cms.x, guided_vel_target_cms.y};
    Vector2f_t guided_accel_target_cmss_xy = {guided_accel_target_cmss.x, guided_accel_target_cmss.y};

    posctrl_input_vel_accel_xy(mode->pos_control, &guided_vel_target_cms_xy, &guided_accel_target_cmss_xy, false);
    if (!mguided_stabilizing_vel_xy() && !do_avoid) {
        // set position and velocity errors to zero
        posctrl_stop_vel_xy_stabilisation(mode->pos_control);
    } else if (!mguided_stabilizing_pos_xy() && !do_avoid) {
        // set position errors to zero
        posctrl_stop_pos_xy_stabilisation(mode->pos_control);
    }
    posctrl_input_vel_accel_z(mode->pos_control, &guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);

    // call velocity controller which includes z axis controller
    posctrl_update_xy_controller(mode->pos_control);
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t posctrl_thrust_vector = posctrl_get_thrust_vector(mode->pos_control);

    // call attitude controller
    if ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_HOLD) {
        // roll & pitch from position controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, target_yaw_rate);
    } else if ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_RATE) {
        // roll & pitch from position controller, yaw rate from mavlink command or mission item
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_rate_cds(mode->auto_yaw));
    } else {
        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_yaw(mode->auto_yaw), autoyaw_rate_cds(mode->auto_yaw));
    }
}

// pause_control_run - runs the guided mode pause controller
// called from guided_run
static void mguided_pause_control_run(mode_base_t mode)
{
    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        // do not spool down tradheli when on the ground with motor interlock enabled
        // copter.is_tradheli() && motors->get_interlock()
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // set the horizontal velocity and acceleration targets to zero
    Vector2f_t vel_xy = VECTOR2F_DEFAULT_VALUE;
    Vector2f_t accel_xy = VECTOR2F_DEFAULT_VALUE;
    posctrl_input_vel_accel_xy(mode->pos_control, &vel_xy, &accel_xy, false);

    // set the vertical velocity and acceleration targets to zero
    float vel_z = 0.0;
    posctrl_input_vel_accel_z(mode->pos_control, &vel_z, 0.0f, false, false);

    // call velocity controller which includes z axis controller
    posctrl_update_xy_controller(mode->pos_control);
    posctrl_update_z_controller(mode->pos_control);

    // call attitude controller
    Vector3f_t posctrl_thrust_vector = posctrl_get_thrust_vector(mode->pos_control);

    attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, 0.0f);
}

// posvelaccel_control_run - runs the guided position, velocity and acceleration controller
// called from guided_run
static void mguided_posvelaccel_control_run(mode_base_t mode)
{
    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!fms.failsafe.radio && mode_use_pilot_yaw(mode)) {
        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(mode->channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        // do not spool down tradheli when on the ground with motor interlock enabled
        // copter.is_tradheli() && motors->get_interlock()
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // set velocity to zero and stop rotating if no updates received for 3 seconds
    uint32_t tnow = time_millis();
    if (tnow - update_time_ms > mguided_get_timeout_ms()) {
        vec3_zero(&guided_vel_target_cms);
        vec3_zero(&guided_accel_target_cmss);
        if (((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_RATE) || ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_ANGLE_RATE)) {
            autoyaw_set_rate(mode->auto_yaw, 0.0f);
        }
    }

    // send position and velocity targets to position controller
    if (!mguided_stabilizing_vel_xy()) {
        // set the current commanded xy pos to the target pos and xy vel to the desired vel
        guided_pos_target_cm.x = posctrl_get_pos_target_cm(mode->pos_control).x;
        guided_pos_target_cm.y = posctrl_get_pos_target_cm(mode->pos_control).y;
        guided_vel_target_cms.x = posctrl_get_vel_desired_cms(mode->pos_control).x;
        guided_vel_target_cms.y = posctrl_get_vel_desired_cms(mode->pos_control).y;
    } else if (!mguided_stabilizing_pos_xy()) {
        // set the current commanded xy pos to the target pos
        guided_pos_target_cm.x = posctrl_get_pos_target_cm(mode->pos_control).x;
        guided_pos_target_cm.y = posctrl_get_pos_target_cm(mode->pos_control).y;
    }

    Vector2f_t guided_pos_target_cm_xy = {guided_pos_target_cm.x, guided_pos_target_cm.y};
    Vector2f_t guided_vel_target_cms_xy = {guided_vel_target_cms.x, guided_vel_target_cms.y};
    Vector2f_t guided_accel_target_cmss_xy = {guided_accel_target_cmss.x, guided_accel_target_cmss.y};

    posctrl_input_pos_vel_accel_xy(mode->pos_control, &guided_pos_target_cm_xy, &guided_vel_target_cms_xy, &guided_accel_target_cmss_xy, false);
    if (!mguided_stabilizing_vel_xy()) {
        // set position and velocity errors to zero
        posctrl_stop_vel_xy_stabilisation(mode->pos_control);
    } else if (!mguided_stabilizing_pos_xy()) {
        // set position errors to zero
        posctrl_stop_pos_xy_stabilisation(mode->pos_control);
    }

    // guided_pos_target z-axis should never be a terrain altitude
    if (guided_pos_terrain_alt) {
        //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
    }

    float pz = guided_pos_target_cm.z;
    posctrl_input_pos_vel_accel_z(mode->pos_control, &pz, &guided_vel_target_cms.z, guided_accel_target_cmss.z, false);
    guided_pos_target_cm.z = pz;

    // run position controllers
    posctrl_update_xy_controller(mode->pos_control);
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t posctrl_thrust_vector = posctrl_get_thrust_vector(mode->pos_control);

    // call attitude controller
    if ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_HOLD) {
        // roll & pitch from position controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, target_yaw_rate);
    } else if ((enum autopilot_yaw_mode)mode->auto_yaw->_mode == AUTO_YAW_RATE) {
        // roll & pitch from position controller, yaw rate from mavlink command or mission item
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_rate_cds(mode->auto_yaw));
    } else {
        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_yaw(mode->auto_yaw), autoyaw_rate_cds(mode->auto_yaw));
    }
}

// helper function to set yaw state and targets
static void mguided_set_yaw_state(mode_base_t mode, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{
    if (use_yaw && relative_angle) {
        autoyaw_set_fixed_yaw(mode->auto_yaw, yaw_cd * 0.01f, 0.0f, 0, relative_angle);
    } else if (use_yaw && use_yaw_rate) {
        autoyaw_set_yaw_angle_rate(mode->auto_yaw, yaw_cd * 0.01f, yaw_rate_cds * 0.01f);
    } else if (use_yaw && !use_yaw_rate) {
        autoyaw_set_yaw_angle_rate(mode->auto_yaw, yaw_cd * 0.01f, 0.0f);
    } else if (use_yaw_rate) {
        autoyaw_set_rate(mode->auto_yaw, yaw_rate_cds);
    } else {
        autoyaw_set_mode_to_default(mode->auto_yaw, false);
    }
}

static ModeNumber mguided_number(mode_base_t mode) { return GUIDED; }
static const char *mguided_name(mode_base_t mode) { return "GUIDED"; }
static const char *mguided_name4(mode_base_t mode) { return "GUID"; }
static bool mguided_requires_GPS(mode_base_t mode) { return true; }
static bool mguided_has_manual_throttle(mode_base_t mode) { return false; }
static bool mguided_is_autopilot(mode_base_t mode) { return true; }
static bool mguided_has_user_takeoff(mode_base_t mode, bool must_navigate) { return true; }
static bool mguided_in_guided_mode(mode_base_t mode) { return true; }
static bool mguided_requires_terrain_failsafe(mode_base_t mode) { return true; }

// returns true if pilot's yaw input should be used to adjust vehicle's heading
static bool mguided_use_pilot_yaw(mode_base_t mode) { return (fms.g.guided_options & (uint32_t)GuidedIgnorePilotYaw) == 0; }

// pause guide mode
static bool mguided_pause(mode_base_t mode)
{
    ((ModeGuided*)mode)->_paused = true;
    return true;
}

// resume guided mode
static bool mguided_resume(mode_base_t mode)
{
    ((ModeGuided*)mode)->_paused = false;
    return true;
}

static uint32_t mguided_wp_distance(mode_base_t mode)
{
    switch(((ModeGuided*)mode)->guided_mode) {
    case GUIDED_WP:
        return 0; //wp_nav->get_wp_distance_to_destination();
    case GUIDED_Pos: {
        Vector2f_t position_xy_cm = {mode->ahrs->relpos_cm.x, mode->ahrs->relpos_cm.y};
        Vector2f_t pos_target_cm = {guided_pos_target_cm.x, guided_pos_target_cm.y};
        return vec2_get_distance_cm(&position_xy_cm, &pos_target_cm);
        }
    case GUIDED_PosVelAccel:
        return posctrl_get_pos_error_xy_cm(mode->pos_control);
        break;
    default:
        return 0;
    }
}

static int32_t mguided_wp_bearing(mode_base_t mode)
{
    switch(((ModeGuided*)mode)->guided_mode) {
    case GUIDED_WP:
        return 0; //wp_nav->get_wp_bearing_to_destination();
    case GUIDED_Pos: {
        Vector2f_t position_xy_cm = {mode->ahrs->relpos_cm.x, mode->ahrs->relpos_cm.y};
        Vector2f_t pos_target_cm = {guided_pos_target_cm.x, guided_pos_target_cm.y};

        return vec2_get_bearing_cd(&position_xy_cm, &pos_target_cm);
        }
    case GUIDED_PosVelAccel:
        return posctrl_get_bearing_to_target_cd(mode->pos_control);
        break;
    case GUIDED_TakeOff:
    case GUIDED_Accel:
    case GUIDED_VelAccel:
    case GUIDED_Angle:
        // these do not have bearings
        return 0;
    }
    // compiler guarantees we don't get here
    return 0.0;
}

static float mguided_crosstrack_error(mode_base_t mode)
{
    switch (((ModeGuided*)mode)->guided_mode) {
    case GUIDED_WP:
        return 0.0f; //wp_nav->crosstrack_error();
    case GUIDED_Pos:
    case GUIDED_TakeOff:
    case GUIDED_Accel:
    case GUIDED_VelAccel:
    case GUIDED_PosVelAccel:
        return posctrl_crosstrack_error(mode->pos_control);
    case GUIDED_Angle:
        // no track to have a crosstrack to
        return 0;
    }
    // compiler guarantees we don't get here
    return 0;
}

/*------------------------------------test------------------------------------*/


